//
//  Camera.c
//  Vector_Math
//
//  Created by John R Peterson on 3/14/12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//

#include <stdio.h>
#include <stdio.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>

#include "../imagesource/image_source.h"
#include "../common/imageconvert.h"
#include "../common/image_u8.h"
#include "../common/image_u32.h"
#include "../common/timestamp.h"
#include "LinAlg.h"

Vec* Camera_intersectGround(Vec *p);
VecList* Camera_intersectGroundMany(VecList *vlist);

int main(int argc, char * argv[])
{
    char *url = NULL;
    char const *outdir = NULL;

    // open camera
    char **urls = image_source_enumerate();
    printf("Cameras:\n");
    for (int i = 0; urls[i] != NULL; i++)
        printf("  %3d: %s\n", i, urls[i]);

    if (urls[0]==NULL) {
        printf("Found no cameras.\n");
        return -1;
    }

    url = urls[0];

    if (argc > 0) {
        outdir = argv[1];
	printf("hrmm\n %s \n", outdir);
    }

    // open the image source
    image_source_t *isrc = image_source_open(url);
    if (isrc == NULL) {
        printf("Error opening device.\n");
        return -1;
    }

    // enumerate its options
    for (int i = 0; i < isrc->num_formats(isrc); i++) {
        image_source_format_t *ifmt = isrc->get_format(isrc, i);
        printf("%3d: %4d x %4d (%s)\n", i, ifmt->width, ifmt->height, ifmt->format);
    }
    // choose first option
    int format = 0;
    isrc->set_format(isrc, format);

    if (1) {
        int res = isrc->start(isrc);
        printf("start: res = %d\n", res);
    }

    int nframes = 0;

//    setlinebuf(stdout);
    image_source_format_t *ifmt = isrc->get_format(isrc, format);
    image_u32_t * rgb_full = image_u32_create(ifmt->width, ifmt->height);
    image_u32_t * rgb_half = image_u32_create(ifmt->width/2, ifmt->height/2);
    image_u8_t * gray_full = image_u8_create(ifmt->width, ifmt->height);

    uint64_t last_end= 0;
    while(1) {
        void *imbuf = NULL;
        int imbuflen = 0;


        int res = isrc->get_frame(isrc, &imbuf, &imbuflen);
        if (res < 0) {
            printf("get_frame fail: %d\n", res);
            continue;
        } else {
            nframes++;
        }

        // Step 0: Convert byte buffer to image_u8_t
        image_u8_t * bayer = malloc(sizeof(image_u8_t));
        bayer->width = ifmt->width;
        bayer->height = ifmt->height;
        bayer->stride = ifmt->width*2; // two bytes per pixel
        bayer->buf = (uint8_t*)imbuf;

        int64_t start = timestamp_now();
        // Step 1: Debayer
        debayer_GBRG(bayer, rgb_full);
        debayer_GBRG_half(bayer, rgb_half);
        image_u8_fill_gray_from_rgb_u32(gray_full,rgb_full);

        if (outdir != NULL) {
            char  filename[256];
            sprintf(filename,"%s/full.ppm",outdir);
            image_u32_write_pnm(rgb_full,filename);

            sprintf(filename,"%s/half.ppm",outdir);
            image_u32_write_pnm(rgb_half, filename);

            sprintf(filename,"%s/full.pgm",outdir);
            image_u8_write_pnm(gray_full, filename);
        }

        int64_t end = timestamp_now();

        uint64_t diff_process = (end - start);
        uint64_t diff_loop = (end - last_end);

        printf("get_frame: res = %d count = %10d (%10d bytes)  process rate: %.2f  Hz loop rate: %.2f Hz\r",
               res, nframes, imbuflen, 1E6/(diff_process),1E6/(diff_loop));
        fflush(stdout);
        isrc->release_frame(isrc, imbuf);
        last_end = end;
    }
    printf("\n");
    fflush(stdout);
    isrc->close(isrc);
    return 1;
}




/* estimated camera calibration matrix 
 * f = 583 or 596 about that in units of pixels per meter
 * [   583   0   648    0]
 * [    0   583  480    0]
 * [    0    0    0     1]
 */

// TODO undistort a pixel, go from distored image coordinates to 
// undistorted image coordinates
Vec *Camera_undistort(Vec *p) {
    
}

// TODO project point from image coordinates into 3D coordinates
// converts a pixel in image coordinates to local 3D coordinates by 
// intersecting the ray with the ground plane

// remember that x image coordinate are x downwards and y to the right
// z back into the camera
Vec *Camera_intersectGround(Vec *p) {
    float f = 583;
    float cx = 480;   // center of the image in pixels
    float cy = 648;   // may need to modify these numbers when the image is downsampled
    float cameraheight = 0.205; // height of the optical axis above the ground plane
                                // in meters
    
    float k[] = {f, 0, cx, 0, f, cy, 0, 0, 1};
    Matrix *K = Matrix_createV(k, 3, 3);
    
    Matrix *invK = Matrix_inverse(K);
    
    Vec *np = Matrix_multiplyAV(invK,Vec_toHI(p)); // compute normalized image coordinate
    // should output a length 3 vector
    
    // apply rigid body transformation to transform vector from
    // local normalized image coordinates to local world coordinates
    // x forwards, y to the left, z up    
    // when composing rotations, rotations about current frame post multiply
    // then apply translation in local coordinates
    Matrix *R = Matrix_multiplyABDD(Matrix_rotateY(PI/2.0),Matrix_rotateX(PI));
    
    // apply rotation need to go from 3D to 3D homogeneous 
    Vec *nlw = Matrix_multiplyAV(R,Vec_toHI(np));
    
    // scale the vector by this value and then pull off the x and y components
    float sf = cameraheight/(nlw->val[2]);
    Vec_scaleI(nlw,sf);
    
    float groundcord[] = {nlw->val[0], nlw->val[1]};
    Vec *groundp =  Vec_create(groundcord, 2);
    
    // cleanup
    Matrix_destroy(K);
    Matrix_destroy(invK);
    Matrix_destroy(R);
    Vec_destroy(np);
    Vec_destroy(nlw);
    
    return groundp;
}

VecList* Camera_intersectGroundMany(VecList *vlist) {
    float f = 583;
    float cx = 480;   // center of the image in pixels
    float cy = 648;   // may need to modify these numbers when the image is downsampled
    float cameraheight = 0.205; // height of the optical axis above the ground plane
    // in meters
    
    float k[] = {f, 0, cx, 0, f, cy, 0, 0, 1};
    Matrix *K = Matrix_createV(k, 3, 3);
    
    Matrix *invK = Matrix_inverse(K);
    // should output a length 3 vector
    
    // apply rigid body transformation to transform vector from
    // local normalized image coordinates to local world coordinates
    // x forwards, y to the left, z up    
    // when composing rotations, rotations about current frame post multiply
    // then apply translation in local coordinates
    Matrix *R = Matrix_multiplyABDD(Matrix_rotateY(PI/2.0),Matrix_rotateX(PI));
    
    int i;
    VecList *groundpoints = VecList_createS(vlist->size);
    for (i = 0; i < vlist->size; i++) {

        Vec *np = Matrix_multiplyAV(invK,Vec_toHI(vlist->list[i])); // compute normalized image coordinate
        // apply rotation need to go from 3D to 3D homogeneous 
        Vec *nlw = Matrix_multiplyAV(R,Vec_toHI(np));
        // scale the vector by this value and then pull off the x and y components
        float sf = cameraheight/(nlw->val[2]);
        Vec_scaleI(nlw,sf);
        // convert 
        float groundcord[] = {nlw->val[0], nlw->val[1]};
        Vec *groundp =  Vec_create(groundcord, 2);
        VecList_add(groundpoints,groundp);
        
        // clean up some
        Vec_destroy(np);
        Vec_destroy(nlw);
    }
    
    // cleanup
    Matrix_destroy(K);
    Matrix_destroy(invK);
    Matrix_destroy(R);
    
    return groundpoints;
}
